#ifndef	_FUNCDEF_H_
#define _FUNCDEF_H_



#include <std.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <idl.h>
#include <sem.h>
#include <tsk.h>
#include <ecm.h>
#include <log.h>
#include <swi.h>
#include <gio.h>
#include <gbl.h>
#include <clk.h>
#include <que.h>

#include "C6747.h"
#include "ti/pspiom/uart/Uart.h"
#include "ti/pspiom/i2c/I2c.h"
#include "ti/pspiom/psc/Psc.h"
#include "ti/pspiom/platforms/evm6747/I2c_evmInit.h"
#include "ti/pspiom/platforms/evm6747/Uart_evmInit.h"
#include "../inc/dsplib.h"


#include "..\libNav\common.h"
#include "..\libNav\err.h"
#include "..\libNav\BMA180.h"
#include "..\libNav\ITG3205.h"
#include "..\libNav\MS5611.h"
#include "..\libNav\HMC5883L.h"
#include "..\libNav\GPS.h"

#include "..\libMath\matrix.h"
#include "..\libNav\navbase.h"
#include "..\libNav\AHRS.h"

#ifdef __cplusplus
extern "C"
{
#endif


/* ========================================================================== */
/*                         Common Data Structures           		          */
/* ========================================================================== */
extern LOG_Obj sys_trace;
extern LOG_Obj Term_trace;
extern LOG_Obj AccSampleTsk;
extern LOG_Obj GyroSampleTsk;
extern LOG_Obj BaroSampleTsk;
extern LOG_Obj MagSampleTsk;
extern LOG_Obj GpsSampleTsk;


extern SEM_Obj datalog_sem;
extern SEM_Obj HMC5883L_sem;
extern SEM_Obj MS5611_sem;
extern SEM_Obj ITG3200_sem;
extern SEM_Obj BMA180_sem;
extern SEM_Obj ctrl_term_sem;
extern SEM_Obj GPS_sem;
extern SEM_Obj matrix_sem;
extern SEM_Obj UART2_OUT_SEM;

extern STS_Obj sts_BMA180;
extern STS_Obj sts_HMC5883L;
extern STS_Obj sts_ITG3205;
extern STS_Obj sts_MS5611;
extern STS_Obj sts_GPS;
extern STS_Obj sts_MatrixInv;

/*
* The queue is created by the Configuration Tool.
* For simplicity, we use MEM_alloc and MEM_free to manage
* the MsgObj structures. It would be way more efficient to
* preallocate a pool of MsgObj's and keep them on a 'free'
* queue. Using the Config Tool, create 'freeQueue'. Then in
* main, allocate the MsgObj's with MEM_alloc and add them to
* 'freeQueue' with QUE_put.
* You can then replace MEM_alloc calls with QUE_get(freeQueue)
* and MEM_free with QUE_put(freeQueue, msg).
*
* A queue can hold an arbitrary number of messages or elements.
* Each message must, however, be a structure with a QUE_Elem as
* its first field.
* */
extern QUE_Obj QUE0;
extern QUE_Obj QUE1;
extern QUE_Obj QUE2;
extern QUE_Obj QUE3;
extern QUE_Obj QUE4;
extern QUE_Obj QUE5;
extern QUE_Obj QUE6;


//number of sensors: ITG3205/MS5611/HMC5883L/BMA180
#define NUM_SENS		4
//TSKs' number
#define	TSK_BMA180		0
#define	TSK_ITG3205		1
#define	TSK_HMC5883L	2
#define TSK_MS5611		3
#define	TSK_datalog		4
#define TSK_ctrl_term	5
#define TSK_GPS			6
#define MATRIX_TEST     7
#define NUM_TSKS 		8

#define SYS_TIME_OUT		200	/* Timeout value by system clock ticks (0.1 ms/tick) */


#define MAG_SET_MODE_CMD	1
#define MAG_SET_RANGE_CMD	2

#define ACC_SET_BW			3
#define ACC_SET_RANGE		4

#define TXT_DATA_LOG			0
#define BIN_DATA_LOG			1
#define FULL_SET_SCALE_DATA_LOG	2
#define SYSTEM_PROFILING		3
#define GPS_DATA_LOG			4

//EKF mode
#define AC 		1
#define ACMG 	2

extern int DATA_LOG_TYPE;
extern Uint8 ACC_CURRENT_SCALE_INDEX;
extern Uint8 MAG_CURRENT_SCALE_INDEX;
extern double imu_info[14];
extern double timelabel_pre;


typedef enum time_op {
	SET_PREV_TIME = 0,
	DELTA_TIME = 1,
	RESET_TIME = 2
}time_op;

typedef struct NavDataObj {
	short part1,part2,part3,part4;
	//BMA180
	double BMA180_x,BMA180_y,BMA180_z,BMA180_T,roll,pitch;
	short rBMA180_x,rBMA180_y,rBMA180_z,rBMA180_T;
	//ITG3200
	double Gyro_x,Gyro_y,Gyro_z,Gyro_T;
	short rGyro_x,rGyro_y,rGyro_z,rGyro_T;
	//HMC5883L
	double Mag_x,Mag_y,Mag_z,azimuth;
	short rMag_x,rMag_y,rMag_z;
	//MS5611
	double P,T,BAROH,BAROH_R,P_offset,T_offset,H_offset;
	//uBlox GPS
	GPS_param gps_data;
	NAVSOL GPS_BUF;

} NavDataObj, *NavData;

/* Internal data structure in .data section*/
extern NavDataObj vNavData;




typedef struct MsgObj {
	QUE_Elem elem; 	/* first field for QUE - system default, never modify! */
	Int32 sender;	/* Task ID (sender)  - set by sender */
	QUE_Handle receipt; /* sender's queue - provide by sender */
	Int32 cmd_1; 		/* message value:cmd - set by sender */
	Int32 cmd_2; 		/* message value:cmd - set by sender */
	Int32 status;		/* status - set by receiver */
	SEM_Handle sem;	/* semaphore for synchronous operation -  provide by sender */
} MsgObj, *Msg;



typedef struct TSK_Ctrl_t{
	QUE_Handle		queue;			/* task message queue IPC */
	LOG_Handle		trace;			/* BIOS Log Object associated with task */
	State_t 		status;			/* Task self status */
	GIO_Handle		inhd;			/* general input channel */
	GIO_Handle		outhd;			/* general output channel */
	char 			pbuf[512];		/* serial message output buffer */
	size_t 			len;			/* Actual string length of serial message output buffer */

	/* Time for system profiling */
	unsigned long	prev_t;		/* Previous time value for benchmarking */
	unsigned long	count;
	unsigned long	total_t;
	double			avg_t;	/* Average runtime for benchmarking */
}TSK_Ctrl_b, *TSK_Ctrl_b_p;



/* System Control Block */
typedef struct SYS_Ctrl_t{
	/* System Internal Variables */
	LOG_Obj	*			trace;				//BIOS Log Object associated with syste
	Bool 				INITIALISED;		//System Initialization State
	/* Tasks Variables */
	Bool 				TSK_ENA[NUM_TSKS];	//PRD Tasks Enabling/Disabling
	TSK_Ctrl_b 			TCB[NUM_TSKS];		//Task Control Block
	Sensor_Param 		sensor[NUM_SENS];			/* Sensor parameters */
	/* PRD Timer Variables */
	unsigned long		TIME_BASE; 			//STSEM GPS TIME BASE RECORDED SINCE LAST GPS TIME UPDATE
	unsigned long 		TIME_STAMP;			//SYSTEM TIME STAMP
	unsigned short 		TIME_STAMP_H;
	unsigned short 		TIME_STAMP_L;
	unsigned long 		GPS_TIME_BASE;		//GPS TIME STAMP BASE

	unsigned long 		SYSTEM_TIME;		//SYSTEM TIME STAMP
	unsigned short 		SYSTEM_TIME_H;
	unsigned short		SYSTEM_TIME_L;

	unsigned long 		CLK_period;			//the number of high-resolution timer counts per low resolution interrupt.
	unsigned long 		CLK_pms;			//the number of high-resolution timer counts per millisecond
}SYS_Ctrl_b,*SYS_Ctrl_b_p;
extern SYS_Ctrl_b SCB;


void vTskTime(TSK_Ctrl_b *tsk, time_op op);
unsigned long vUpdateTimeStamp(void);
void central_control_PRD(void);
void navcon_start(void);
void user_i2c_init(void);
void user_uart2_init(void);

/*
 * Perform essential configuration for task with TSK_ID
 */
Int32 register_tsks(Int32 TSK_ID, LOG_Handle trace, QUE_Handle queue, String dev, Int32 dir, Ptr chanParams);


#ifdef __cplusplus
}
#endif /* extern "C" */



#endif
